#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <modbus/modbus.h>
#include <cstring>
#include <math.h>




int NUMBER_EACH_SEND = 10;

void write_reg(uint16_t joint_data[][12],int joint_data_num, modbus_t *ctx, uint16_t reg_address){
    int rc;
    int loc;
    int size_of_vector = floor(joint_data_num / NUMBER_EACH_SEND);
    std::cerr << "size_of_vector: " << size_of_vector << "joint_data_num: "  << joint_data_num << "::" << NUMBER_EACH_SEND << std::endl;
    int lastSize =  joint_data_num - size_of_vector*NUMBER_EACH_SEND;
    uint16_t cur_address = reg_address;
    for(int i=0; i<size_of_vector; i++){
        cur_address =  reg_address + i*NUMBER_EACH_SEND*12;
        std::cout << "i= " << i << "cur_address= " << cur_address << std::endl;
        rc = modbus_write_registers(ctx, cur_address, NUMBER_EACH_SEND*12, &joint_data[i*NUMBER_EACH_SEND][0]);

        //std::cout << "i= " << i << "cur_address= " << cur_address << std::endl;
        if (rc == -1) {
            std::cerr << "Write register failed: " << modbus_strerror(errno) << std::endl;
            modbus_close(ctx);
            modbus_free(ctx);

        }
    }

}





// 插值函数
void interpolatePoints(const trajectory_msgs::JointTrajectoryPoint& start,
                       const trajectory_msgs::JointTrajectoryPoint& end,
                       std::vector<trajectory_msgs::JointTrajectoryPoint>& result,
                       int num_points)
{
  result.push_back(start);

  double time_diff = (end.time_from_start - start.time_from_start).toSec();
  double vel_start = 0.0;
  double vel_end = 0.0;

  for (int i = 1; i <= num_points; ++i)
  {
    double ratio = static_cast<double>(i) / static_cast<double>(num_points);
    trajectory_msgs::JointTrajectoryPoint point;
    point.time_from_start = start.time_from_start + ros::Duration(ratio * time_diff);

    // 根据插值点位置调整速度
    double acceleration = (i <= num_points / 4) ? 4.0 : (i > 3 * num_points / 4) ? -4.0 : 0.0;
    double deceleration = -acceleration;
    double max_velocity = 1.0;  // 调整最大速度

    // 计算梯形插值的速度
    double vel = vel_start + acceleration * ratio * (1 - ratio) * max_velocity - deceleration * (1 - ratio) * ratio * max_velocity;

    for (size_t j = 0; j < start.positions.size(); ++j)
    {
      // 进行梯形插值
      point.positions.push_back(start.positions[j] + ratio * (end.positions[j] - start.positions[j]));

      // 设置速度
      point.velocities.push_back(vel);
    }

    result.push_back(point);
  }
}

int main(int argc, char **argv)
{
  modbus_t *ctx;
    int rc;
    const char *ip_address = "192.168.0.70"; // Modbus TCP设备的IP地址
    int port = 502; // Modbus TCP设备的端口号
    uint16_t reg_address = 102; // 寄存器地址
    uint16_t reg_value = 1234; // 要写入的值

    // 创建Modbus连接
    ctx = modbus_new_tcp(ip_address, port);
    if (ctx == NULL) {
        std::cerr << "Unable to create the libmodbus context" << std::endl;
        return -1;
    }

    // 连接Modbus设备
    if (modbus_connect(ctx) == -1) {
        std::cerr << "Connection failed: " << modbus_strerror(errno) << std::endl;
        modbus_free(ctx);
        return -1;
    }

    // 设置超时时间（可选）
    modbus_set_response_timeout(ctx, 2, 0);

     // 写入寄存器数据
     rc = modbus_write_register(ctx, 100, 999);
     if (rc == -1) {
        std::cerr << "Write register failed: " << modbus_strerror(errno) << std::endl;
        modbus_close(ctx);
        modbus_free(ctx);
        return -1;
    } else {
        std::cout << "Write register successful" << std::endl;
    }
  
  ros::init(argc, argv, "moveit_custom_demo");
  ros::NodeHandle node_handle; 
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit::planning_interface::MoveGroupInterface group("jr603arm");

  // 设置机器人回到初始状态 "zero"
  group.setPlanningTime(10.0);
  
  // 设置机器人终端的目标位置
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 0.499998;
  target_pose1.orientation.x = -0.500019;
  target_pose1.orientation.y = -0.499959;
  target_pose1.orientation.z = 0.500024;

  target_pose1.position.x = -0.168002;
  target_pose1.position.y = 0.000004;
  target_pose1.position.z = 0.243004;

  group.setMaxVelocityScalingFactor(0.0001);
  group.setPoseTarget(target_pose1);
  group.setGoalTolerance(0.001);

  // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  moveit::core::MoveItErrorCode success = group.plan(my_plan);

  ROS_INFO("Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

  // 访问规划的轨迹
  moveit_msgs::RobotTrajectory& trajectory = my_plan.trajectory_;

  // 插入额外的点
  std::vector<trajectory_msgs::JointTrajectoryPoint> interpolated_points;
  for (size_t i = 0; i < trajectory.joint_trajectory.points.size() - 1; ++i)
  {
    const trajectory_msgs::JointTrajectoryPoint& start_point = trajectory.joint_trajectory.points[i];
    const trajectory_msgs::JointTrajectoryPoint& end_point = trajectory.joint_trajectory.points[i + 1];

    interpolatePoints(start_point, end_point, interpolated_points, 20);
  }

  // 更新轨迹
  moveit_msgs::RobotTrajectory new_trajectory = my_plan.trajectory_;
  new_trajectory.joint_trajectory.points = interpolated_points;

  // 打印调整后的速度
  float test_data[2500][6];
  uint16_t test_data_int[2500][12];
  for (size_t i = 0; i < new_trajectory.joint_trajectory.points.size(); ++i)
  {
    const trajectory_msgs::JointTrajectoryPoint& point = new_trajectory.joint_trajectory.points[i];
    ROS_INFO("Waypoint %zu:", i);
    for (size_t j = 0; j < point.velocities.size(); ++j)
    {
      ROS_INFO("Vel %zu: %f", j, point.velocities[j]);
      test_data[i][j] = point.velocities[j];
    }
	
  }

  for(int i=new_trajectory.joint_trajectory.points.size(); i<2500; i++){
        for(int j=0; j<6; j++){
            test_data[i][j] = test_data[new_trajectory.joint_trajectory.points.size() - 1][j];
        }
    }
  for(int i=0; i < 2500; i++){
	  ROS_INFO("Waypoint %d:", i);
	  for (int j = 0; j < 6; ++j){
		  ROS_INFO("Vel %d %f", j, static_cast<double>(test_data[i][j]));
	  }
  }
  memcpy(test_data_int, test_data, sizeof(test_data_int));
  write_reg(test_data_int, 2500, ctx, reg_address);   
  // 让机械臂按照规划的轨迹开始运动。
  if (success)
    group.execute(my_plan);

  robot_state::RobotStatePtr current_state = group.getCurrentState();
  // 获取机器人当前末端的位姿
  const Eigen::Isometry3d& end_effector_pose = current_state->getGlobalLinkTransform("link6");
  // 获取位姿的旋转部分（四元数）
  Eigen::Quaterniond quaternion(end_effector_pose.rotation());
  // 打印初始状态的xyzw值
  ROS_INFO("Initial Pose (xyzw): %.3f %.3f %.3f %.3f", quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w());
  ros::shutdown(); 
  return 0;
}
